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1 Introduction 

Simultaneous localization and mapping (SLAM) is a fundamental problem in robotics. It is the 
process of creating a map of the environment while, at the same time, estimating the position and 
attitude of the robot relative to the map. SLAM enables autonomous path planning and control. 

The extended Kalman Filter (EKF), Particle Filters, and Expectation Minimization are among 
the most popular SLAM methods [1,2]. The EKE and the extended Information Eilter (EIE) 
involve propagating a covariance matrix, or its inverse as in the EIE, along with the states of the 
robot and map. This leads to computational inefficiency for large scale problems where the number 
of landmarks may be very large. Moreover, the process and measurement noise are assumed to be 
Gaussian within the EKE formulation. This may not be a problem for robotic systems equipped 
with high quality sensors. However, low-cost inertial measurement units (IMUs) are plagued by high 
levels of non-Guassian noise and biases [3]. Consequently, the EKE SLAM algorithm can be difficult 
to apply to robotic systems equipped with low-cost IMUs. In addition, inherent linearization in 
the EKE formulation can lead to computational difficulty as well. 

We present a differential geometric SLAM (DG-SLAM) algorithm that evolves directly on the 
special Euclidean group, SE{3). The proposed filter employs methods from differential geometry 
to propagate the state and map estimates. Differential geometric methods have been employed 
previously in the literature to the problems of state estimation on 50(3) and SE{3) [3,4]. Unlike 
EKE SLAM, the proposed filter is provably asymptotically stable. That is to say that, in the absence 
of bias and noise, the estimated states are guaranteed to converge to the true states. Another 
advantage of the algorithm is the absence of matrix inversions, which makes the algorithm suitable 
for large-scale implementation. By approaching the SLAM problem in a geometric framework we 
hope to avoid the pitfalls of traditional SLAM techniques. In particular, we hope that the proposed 
algorithm is robust to non-Gaussian noise associated with low-cost sensors. 

2 Applications 

As shown in Eigure 1 the proposed DG-SLAM algorithm can be used to build a map and simulta¬ 
neously localize a vehicle moving in three dimensional space in a variety of environments. 

3 Notation 

We consider the simultaneous mapping and localization problem in three dimensions. Eirst let Ea 
and Eb denote the body-fixed frame of the robot and the datum frame, respectively, as shown in 
Eigure 2. The goal is to estimate the pose of the robot and the landmark positions, relative to 
the datum frame. Let r denote the position of the center of mass of the vehicle relative to the 
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Figure 1: UAVs, terrestrial robots, submersibles. Credit: MAAV (http://www.niaavumich.org), 
MRover (http://www.umrover.org), PeRL (http://robots.engin.umich.edu). 


origin of the datum frame. Also, let p *, i G {1,..., £} denote the position of landmark i relative 
to the origin of the datum frame. Vectors and p * may be resolved in any reference frame. For 
example, the representation of and p Mn 7a is denoted Va and Pa, where the subscript denotes 

the frame in which the vector has been resolved. An additional vector, denoted denotes the 
position of landmark i relative to the center of mass of the vehicle. 

The pose of the robot is described by X G 5i?(3), where 


SE{3) 


C r 
0 1 


G I C G C'^C = 1 , det(C) 


+l,r G 


Let 



G SE{3), 


( 1 ) 


denote the true pose of the robot. The direction cosine matrix Cba maps the coordinates of a vector 
resolved in 7a to being resolved in 7),. R is a representation of the attitude of the robot. The 
estimate of the pose of the robot is 


X = 



G SE{3), 


( 2 ) 


where Cea is the estimate of C^a and Ta is the estimate of Va- Similarly, the true map is denoted 
(Pa>Pa> • • • > pi)> while the map estimate is denoted (p^, p^, ..., p^). 


4 Differential Geometric SLAM Formulation 

The following measurements are assumed available: (1) angular velocity, uf, (2) velocity, v^, and 
(3) the position of each landmark relative to the robot, s^’^. All measurements are taken in the 
body frame of the vehicle 7,. For the purpose of the DG-SLAM formulation and the stability 
proof, noise and bias associated with measurements is assumed zero. 
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Figure 2: Vehicle, landmarks, and reference frames. 


The proposed filter takes the following form: 

X = XA, A = 

Pi = CLo:i> 


V 

0 0 


where 


u = ul- kit, 

I 

V = + (a> - ulYCeaXa + A:2 ^ S* - A:3(Ceara + S^’^), 


2=1 


OLi = {u> - ulYCeaV\ - k2S\ 


and s® = Cea(Pa ~^a) ~ Sft is the innovation, which incorporates the error in the map and the pose. 


5 Stability Results 

By employing the Lyapunov function (candidate), 

e 

V(X,pl,...,pi) = i||l-X||2+J]||p®||2, 

2=1 

it can be shown that X —)> 1 and p)j —>■ 0 as t —)> oo, where X = XX“^ denotes the error in the 
pose, and p^ = CeaPa ~ CbaPa denotes the error in the map. In the proof, it is assumed that Cba is 
known, however this is not true in practise. A similar problem occurs in similar 50(3) and SE{3) 
filters [3,4]. In the works of [3,4] a geometric approximation of Cba, C^^ = Cj^(s^’^, s^), is constructed 
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from measurements and used in place of Cha- In the proposed algorithm, is constructed from 
landmark measurements and the estimated landmark positions (i.e., Ta)). When 

using Pq—Tq), the map estimate does not converge to the true map resolved in Ta- However, 

the estimated positions of the landmarks relative to the estimated pose of the robot converges to 
their true counterparts. This is to be expected, since the global SLAM problem is unobservable 
unless at least two landmarks positions are known [5]. 

6 Simulation Results 

The DG-SLAM algorithm is employed in simulation. Although the stability proof assumes no noise, 
to assess robustness of the proposed DG-SLAM algorithm, noise has been added to the angular 
velocity, velocity, and vector measurements in the simulation. The results of the simulation can be 
found in Figures 3 and 4. 






Figure 3: Visualization of SLAM algorithm. The blue diamonds denote the true positions of the 
landmarks. The orange markers denote the estimated landmark positions. The blue and orange 
boxes denote the true and estimated pose of the robot, respectively. During an initial period of 
5 seconds the pose and map estimates rapidly converge towards the true pose and map. After 20 
seconds the errors in the pose and map estimates have approached zero. 
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Figure 4: Simulation results. The error in the pose of the robot as well as the map approaches 
zero. 


7 Conclusions 

The proposed SLAM algorithm is based on differential geometric principles and is similar to SO{3) 
and SE(3) estimators found in the literature. The algorithm requires no matrix inversions making 
it suitable for large scale implementation. Moreover, the filter is guaranteed asymptotically stable, 
assuming no measurement noise, no bias, and Cba is known. 
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